#pragma once
#include "vector3.h"
#include "constants.h"

namespace krryn{
namespace math{
	class matrix{
	public:
		enum{
			TX=3, 
			TY=7, 
			TZ=11, 
			D0=0, D1=5, D2=10, D3=15, 
			SX=D0, SY=D1, SZ=D2, 
			W=D3 
		};

		matrix(){
			identity();
		}

		matrix(const matrix &a_Matrix){
			for(int i = 0; i < 16; i++)
				cell[i] = a_Matrix.cell[i];
		}

		float& operator[](int a_N){
			return cell[a_N];
		}

		void identity(){
			cell[1] = cell[2] = cell[TX] = cell[4] = cell[6] = cell[TY] =
				cell[8] = cell[9] = cell[TZ] = cell[12] = cell[13] = cell[14] = 0;
			cell[D0] = cell[D1] = cell[D2] = cell[W] = 1;
		}

		void init(vector3f a_Pos, float a_RX, float a_RY, float a_RZ){
			matrix t;
			t.rotate_x(a_RZ);
			rotate_y(a_RY);
			concatenate(t);
			t.rotate_z(a_RX);
			concatenate(t);
			translate(a_Pos);
		}

		void rotate_x(float a_RX){
			float sx =(float)sin(a_RX * PI / 180);
			float cx =(float)cos(a_RX * PI / 180);
			identity();
			cell[5] = cx, cell[6] = sx, cell[9] = -sx, cell[10] = cx;
		}

		void rotate_y(float a_RY){
			float sy =(float)sin(a_RY * PI / 180);
			float cy =(float)cos(a_RY * PI / 180);
			identity();
			cell[0] = cy, cell[2] = -sy, cell[8] = sy, cell[10] = cy;
		}

		void rotate_z(float a_RZ){
			float sz =(float)sin(a_RZ * PI / 180);
			float cz =(float)cos(a_RZ * PI / 180);
			identity();
			cell[0] = cz, cell[1] = sz, cell[4] = -sz, cell[5] = cz;
		}

		vector3f get_translation() const{
			return vector3f(cell[TX], cell[TY], cell[TZ]);
		}

		void translate(vector3f a_Pos){
			cell[TX] += a_Pos.m_X; cell[TY] += a_Pos.m_Y; cell[TZ] += a_Pos.m_Z;
		}

		void set_translation(vector3f a_Pos){
			cell[TX] = a_Pos.m_X; cell[TY] = a_Pos.m_Y; cell[TZ] = a_Pos.m_Z;
		}

		void concatenate(matrix& m2){
			matrix res;
			int c;
			for(c = 0; c < 4; c++) for(int r = 0; r < 4; r++)
				res.cell[r * 4 + c] = cell[r * 4] * m2.cell[c] +
				cell[r * 4 + 1] * m2.cell[c + 4] +
				cell[r * 4 + 2] * m2.cell[c + 8] +
				cell[r * 4 + 3] * m2.cell[c + 12];
			for(c = 0; c < 16; c++) cell[c] = res.cell[c];
		}

		vector3f transform(const vector3f& v) const{
			float x  = cell[0] * v.m_X + cell[1] * v.m_Y + cell[2] * v.m_Z + cell[3];
			float y  = cell[4] * v.m_X + cell[5] * v.m_Y + cell[6] * v.m_Z + cell[7];
			float z  = cell[8] * v.m_X + cell[9] * v.m_Y + cell[10] * v.m_Z + cell[11];
			return vector3f(x, y, z);
		}

		void invert(){
			matrix t;
			int h, i;
			float tx = -cell[3], ty = -cell[7], tz = -cell[11];
			for(h = 0; h < 3; h++) for(int v = 0; v < 3; v++) t.cell[h + v * 4] = cell[v + h * 4];
			for(i = 0; i < 11; i++) cell[i] = t.cell[i];
			cell[3] = tx * cell[0] + ty * cell[1] + tz * cell[2];
			cell[7] = tx * cell[4] + ty * cell[5] + tz * cell[6];
			cell[11] = tx * cell[8] + ty * cell[9] + tz * cell[10];
		}

		matrix transpose() const{
			matrix m;

			m[0] = cell[0];	 m[4] = cell[1];  m[8]  = cell[2];  m[12]  = cell[3];
			m[1] = cell[4];	 m[5] = cell[5];  m[9]  = cell[6];  m[13]  = cell[7];
			m[2] = cell[8];	 m[6] = cell[9];  m[10] = cell[10]; m[14]  = cell[11];
			m[3] = cell[12]; m[7] = cell[13]; m[11] = cell[14]; m[15]  = cell[15];
			
			return m;
		}

		float cell[16];
	};
}
}
